perm filename TCALC.SAI[AL,HE] blob
sn#501018 filedate 1980-04-25 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00020 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 IFCR ¬DECLARATION(EXTENDED_COMPILATION)
C00004 00003 ! Load module requirements, declarations
C00006 00004 ! SPLANVAL, VPLANVAL, PPLANVAL
C00009 00005 ! Small utilities: PLACESOL, DEVBITS
C00015 00006 ! TRJCLC
C00025 00007 ! Segment time calculator: RUNTIME
C00028 00008 ! CUBSPL, the polynomial spliner - from de Boor's "A Practical Guide to Splines"
C00033 00009 ! Main body of TRJCLC starts here
C00036 00010 ! Initialize the first node of the motion
C00041 00011 ! Put intermediate points into the thread
C00047 00012 ! Treat the approach
C00054 00013 ! Check for overall time constraints. Fulfil them if possible
C00059 00014 ! Set up the polynomials for any deproach segments
C00062 00015 ! Call the polynomial generator on chunks of the motion.
C00065 00016 ! Compute the gravity and inertia terms
C00066 00017 ! Output the motion table
C00077 00018 ! Reclaim all the arrays in the motion thread
C00078 00019 ! CENTCLC, STOPCLC
C00081 00020 ! Bugs
C00082 ENDMK
C⊗;
IFCR ¬DECLARATION(EXTENDED_COMPILATION)
THENC
ENTRY;
BEGIN "tcalc"
IFCR ¬DECLARATION(CREFFING) THENC DEFINE CREFFING = "FALSE"; ENDC
IFCR ¬ CREFFING THENC
COMMENT: Source file requirements;
REQUIRE "ABBREV.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "MACROS.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[AL,HE]" SOURCE_FILE;
REQUIRE "ARITH.HDR[AL,HE]" SOURCE_FILE ;
REQUIRE "ALREC.SAI[AL,HE]" SOURCE_FILE ;
REQUIRE "INTDEF.SAI[AL,HE]" SOURCE_FILE;
ENDC
REQUIRE "EMITER.HDR[AL,HE]" SOURCE_FILE;
REDEFINE $$PRGID "[]" = ["TCALC"];
ENDC
! Load module requirements, declarations;
REQUIRE "ARMSOL.REL[AL,HE]" LOAD_MODULE;
EXTERNAL SAFE REAL ARRAY LOSTOP, HISTOP, TIMFAC[0:1,1:7];
! First word of array [1:14]. LOSTOP and HISTOP are joint limits,
and TIMFAC is the time in jiffies needed to move one degree (or
inch);
EXTERNAL INTEGER PROCEDURE ARMSOL(REAL ARRAY ANGLE;
RPTR(FRAME,TRANS,SVAL) PTR;INTEGER MECH);
DEFINE DEBUG = "FALSE";
DEFINE YARM_MECH = "'1";
DEFINE YHAND_MECH = "'2";
DEFINE BARM_MECH = "'4";
DEFINE BHAND_MECH = "'10";
DEFINE AHAND_MECH = "'12";
DEFINE ANARM_MECH = "'5";
DEFINE VISE_MECH = "'20";
DEFINE DRIVER_MECH = "'40";
DEFINE YARMSB = "'176000";
DEFINE YHANDSB = "'1000";
DEFINE BARMSB = "'770";
DEFINE BHANDSB = "'4";
DEFINE VISESB = "'2";
DEFINE DRIVERSB = "'1";
! SPLANVAL, VPLANVAL, PPLANVAL;
RPTR(SVAL) PROCEDURE SPLANVAL(RANY OFTHIS);
BEGIN "splanval"
! Returns a scalar as the planning value of this expression;
IF RECTYPE(OFTHIS)=LOC(SVAL)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE COMERR("SPLANVAL garbage",OFTHIS);
END "splanval";
RPTR(V3ECT) PROCEDURE VPLANVAL(RANY OFTHIS);
BEGIN "vplanval"
! Returns a vector as the planning value of this expression;
IF RECTYPE(OFTHIS)=LOC(V3ECT)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE COMERR("VPLANVAL garbage",OFTHIS);
END "vplanval";
RPTR(VALU$) PROCEDURE PPLANVAL (RANY OFTHIS; REFERENCE BOOLEAN SUCCESS);
BEGIN "pplanval"
! Returns a sval or frame as the planning value of this place expression;
SUCCESS ← TRUE;
IF RECTYPE(OFTHIS)=LOC(SVAL)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(FRAME)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(TRANS)
THEN RETURN(OFTHIS)
ELSE IF RECTYPE(OFTHIS)=LOC(DEXPR)
THEN RETURN(DEXPR:VAL[OFTHIS])
ELSE SUCCESS ← FALSE;
RETURN(BPARK); ! The default, to prevent more error messages;
END "pplanval";
! Small utilities: PLACESOL, DEVBITS;
SAFE OWN REAL ARRAY YELLOW,BLUE[1:6];
SIMPLE PROCEDURE INI_POS; ! initialize arm positions;
BEGIN EXTERNAL REAL ARRAY PARK[0:1,1:6];
ARRBLT(YELLOW[1],PARK[0,1],6);ARRBLT(BLUE[1],PARK[1,1],6) END;
REQUIRE INI_POS INITIALIZATION;
PROCEDURE PLACESOL
(REAL ARRAY RES; RPTR(VALU$) PTR; INTEGER MECH; REFERENCE INTEGER FLG);
! PTR points to a frame/trans or a scalar constant.
Its solution is calculated in RES. If armsol has trouble with the
location, then FLG is set TRUE. On the other hand, if it is a
scalar, HANDSOL is called to check bounds (setting FLG) and
to store the result into RES. In any case, MECH specifies
which mechanism (eg BARM) is meant.
;
BEGIN "plcslv"
INTEGER LOJOINT;
LOJOINT ← ARRINFO(RES,1);
IF MECH LAND AHAND_MECH THEN
BEGIN "handsol"
FLG ← ARMSOL(RES,PTR,MECH);
RETURN
END "handsol";
IF MECH = YARM_MECH THEN
BEGIN "yarm"
FLG ← ARMSOL(YELLOW,PTR,YARM_MECH);
ARRBLT(RES[LOJOINT],YELLOW[1],6);
END
ELSE IF MECH = BARM_MECH THEN
BEGIN "barm"
FLG ← ARMSOL(BLUE,PTR,BARM_MECH);
ARRBLT(RES[LOJOINT],BLUE[1],6);
END
ELSE USERERR(0,1,"PLACESOL: unknown device "&CVOS(MECH));
IF FLG THEN PRINT(CRLF & "Joints out of range: ",CVOS(FLG),CRLF);
END "plcslv";
PROCEDURE DEVBITS (REFERENCE INTEGER ARM, SBITS, LOJOINT, HIJOINT; RVAR WHAT);
BEGIN "devbits";
! Takes WHAT as a device name and fills in ARM, SBITS, LOJOINT,
HIJOINT;
IF WHAT = YARM
THEN BEGIN ! Yellow arm;
LOJOINT ← 1;
HIJOINT ← 6;
ARM ← YARM_MECH;
SBITS ← YARMSB;
END
ELSE IF WHAT = YHAND
THEN BEGIN ! Yellow hand;
LOJOINT ← 7;
HIJOINT ← 7;
ARM ← YHAND_MECH;
SBITS ← YHANDSB;
END
ELSE IF WHAT = BARM
THEN BEGIN ! Blue arm;
LOJOINT ← 8;
HIJOINT ← 13;
ARM ← BARM_MECH;
SBITS ← BARMSB;
END
ELSE IF WHAT = BHAND
THEN BEGIN ! Blue hand;
LOJOINT ← 14;
HIJOINT ← 14;
ARM ← BHAND_MECH;
SBITS ← BHANDSB;
END
ELSE IF WHAT = VISE
THEN BEGIN ! Vise;
LOJOINT ← 15;
HIJOINT ← 15;
ARM ← VISE_MECH;
SBITS ← VISESB;
END
ELSE IF WHAT = DRIVER
THEN BEGIN ! Driver;
LOJOINT ← 16;
HIJOINT ← 16;
ARM ← DRIVER_MECH;
SBITS ← DRIVERSB;
END
ELSE BEGIN ! Wrong arm;
COMERR("DEVBITS: No such arm; assuming BLUE.");
LOJOINT ← 8;
HIJOINT ← 13;
ARM ← BARM_MECH;
SBITS ← BARMSB;
END;
END "devbits";
! TRJCLC;
INTERNAL PROCEDURE TRJCLC (RPTR(MOVE$) MOV);
BEGIN "trjclc"
RCLASS TTHREAD (
REAL STIME, UTIME; INTEGER MODE;
RPTR(VARIABLE) EVENT;
RPTR(VARIABLE,VALU$,DEXPR) PLACE;
REAL ARRAY ANGLES, VELS; ! [LOJOINT:HIJOINT];
REAL ARRAY COEFF; ! [1:6,0:5]=[joint,degree] polynomial coefficients;
REAL ARRAY GRAVIN; ! [1:12] gravity, inertia terms for each joint;
RPTR(TTHREAD) NEXT
);
DEFINE TIME_MODE = '3;
DEFINE DEPA_MODE = '4;
DEFINE APPR_MODE = '10;
DEFINE ENDP_MODE = '20;
DEFINE INVI_MODE = '40;
!
Data structures:
A TTHREAD is a linked list of points along which the trajectory
passes. It has these fields:
MODE(INTEGER)
The TIME_MODE bits relate to UTIME:
0:no bound, 1:lower bound, 2:upper bound, 3:exact bound.
DEPA_MODE: on if this point is a departure
APPR_MODE: on if this point is an approach
ENDP_MODE: on if this point is an endpoint (either one)
INVI_MODE: on if this point ends a segment whose time is
inviolate. This applies to the endpoint segments only.
STIME(REAL)
System-calculated time in seconds since previous node. If
there is ia conflict between user and system, then the
resolved time is placed in STIME. That causes problems: The
system time is destroyed, so global resolutions use whatever
foolish thing the user wanted.
UTIME(REAL)
User-supplied time in seconds since previous node.
PLACE(RVAR)
Variable (eventually expression) which has the location of
the mechanism as this node is achieved. This can refer to an
arm (in which case the variable will be a frame) or a hand
(in which case it will be a scalar).
ANGLES(n-VECTOR)
Joint angles for this node, if there is an associated place.
VELS(6-VECTOR)
Joint velocities for this node (deg/sec) if there are some.
EVENT(VARIABLE)
Event to signal to start up code when this node reached.
COEFF(6x6 MATRIX)
The 6 coefficients of the segment ending at this node.
NEXT(PTR(TTHREAD))
Next node.
The trajectory calculator turns motion specifications into
interpretable tables. At the moment it allows any one mechanism,
that is, one arm or one hand. Future work will allow any
combination of mechanisms. The tables are calculated by the
following method:
A thread is made, with a node for each place in the motion
specification, that is, the initial point, the departure, if any,
the via points, the approach point, and the destination. Arm or
hand solutions are calculated for each node. It may be that this
serial calculation will lead to flips of the arm. If this
happens, the proper order is outside-in. This is because the
ARMSOL routine uses the previous solution to resolve ambiguities
in joint 4 of the Scheinman arms.
Any deproach points or calculated via points or calculated
destinations must have code emitted to make a cell for them in
the graph structure. The cell for a departure is marked
permanently invalid. Its calculator uses the hand position
itself, not the place where the arm was to be at the start of the
motion. The cells for the calculated via points and the approach
point are in the graph structure in the usual way. This code
must be emitted at the outermost practical point in the program:
If it is too far in, then it gets redone too often, and if it is
too far out, it might cause graph structure to hang around
associated to non-existent nodes. In any case, it is necessary
to put such code at a block entry, and to be sure to get rid of
the resulting graph structure at block exit. The current code
does not handle any of this.
At this time, the fourth degree polynomials for deproach segments
are calculated, and any given velocity constraints are noted.
The presence of a velocity constraint implies that the
acceleration is constrained to zero. If the user has supplied a
time, it is put in UTIME, and STIME is computed by the system.
If they are compatible, STIME is modified to the final decision
on the time for the segment.
After the entire thread is made, a global check is made to
insure that the timing is in agreement with the user's wishes.
Then the thread is divided into chunks, where each chunk is the
region between two velocity-constrained points (the deproach
points are such). A chunk which has only two points (but not a
deproach chunk, for which the trajectory has already been
calculated) gets a fifth-degree polynomial calculated to match
all the constraints. A chunk with more points requires splining
for the trajectory. The first step is to insert one
unconstrained point in each of the two longest intervals. It has
been found that the best place for these points is almost at one
end of the intervals (.001 of the way to the end) to
minimise overshoot problems. After the fully unconstrained nodes
have been inserted into the thread, the routine POLY is called to
create the coefficients of the third degree splined polynomial.
It has been found that using fourth degree polynomials in two of
the segments instead of inserting two unconstrained points leads
to uncontrollable overshoot. Finally, the resulting trajectory
is emitted.
The following conventions are used for arms and joints. Joints
1-6 are yellow arm (arm 0), and joint 7 is the yellow fingers
(arm 2). Joints 8-13 are the blue arm (arm 1), and joint 14 is
the blue fingers (arm 3). The angle arrays are tailored to have
whatever joints are needed. The arm and hand solution programs
are told which mechanism to expect.
The current code does not check location, velocity or
acceleration bounds except for location bounds at user-specified
places. Instead, location bounds are to a large extent insured
by the servo. Velocity and acceleration can be optimized by
rescaling time, in the cases when the user has not specified any
time in the entire motion, nor any velocities, but this is not
currently attempted.
;
! Segment time calculator: RUNTIME;
INTERNAL REAL SPEED; ! If > 1, motions are slower;
REAL PROCEDURE RUNTIME(REAL ARRAY OLDANG, NEWANG; BOOLEAN ENDSEG(FALSE));
BEGIN "runtim"
! Uses the old and the new joint angles to determine the
correct time in seconds for one segment of motion;
INTEGER JOINT, ! For loop control;
LOJOINT, HIJOINT; ! Defines which arm;
REAL TAU, TTAU, TEMP, DEL;
TAU ← TTAU ← 0;
LOJOINT ← ARRINFO(OLDANG,1);
HIJOINT ← ARRINFO(OLDANG,2);
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN
DEL ← ABS(OLDANG[JOINT]-NEWANG[JOINT]);
TTAU ← MEMORY[LOC(TIMFAC[0,1])-1+JOINT,REAL] * DEL;
IF ENDSEG ∧ DEL < CASE JOINT MOD 7 OF (0.5,15.0,15.0,3.0,15.0,15.0,15.0)
THEN ! Need more time for little motions;
BEGIN
TTAU ← 2 * TTAU;
END;
IF TTAU ≥ TAU THEN TAU ← TTAU;
END;
! If you want to use LOU's model of linear accleration,
you should insert here:
IF (TEMP←.4*SQRT(distance moved in cm.)) > TAU THEN TAU ← TEMP;
! This .4 (sec*sqrt(cm)) is based on experience (LOU
1/13/75), it accounts for the distance moved according
to a linear acceleration model;
TAU ← SPEED * TAU; ! Account for speed_factor specification;
! Convert jiffies → seconds and maybe add some slack time;
IF SPEED ≥ 2 THEN RETURN (TAU/60.0 MAX 0.2)
! No need for any slack time, but make sure segment takes some time;
ELSE RETURN (TAU/60.0 + 0.15 * (2.0 - SPEED)); ! Add on some slack;
! RETURN ((TAU/60.0 + 0.4) MAX .6); ! Used to be just this;
END "runtim";
! CUBSPL, the polynomial spliner - from de Boor's "A Practical Guide to Splines";
PROCEDURE CUBSPL (RPTR(TTHREAD) FIRST, LAST; INTEGER JOINT, nseg);
! Calculate a trajectory for the specified joint using
the thread from FIRST to LAST. The number of segments in the
chunk is given by NS. The location for each node is to be
found in TTHREAD:ANGLES[*][JOINT]. The velocities of the first and
last points are given in TTHREAD:VELS[*][JOINT]. The
timing for each segment is found in TTHREAD:STIME[*] in the
node at the end of the segment. The coefficients of the
resulting polynomial will be stored in the thread nodes, as
TTHREAD:COEFFS[*][JOINT,degree]. ;
BEGIN "cubspl"
SAFE REAL ARRAY c[1:4,1:nseg+1];
INTEGER i,j,l,m,n;
RPTR(TTHREAD) P, Q; ! Used in tracking down the motion thread;
REAL divdf1,divdf3,dtau,g;
n ← nseg+1;
ARRCLR(c);
p ← first;
FOR i ← 1 TIL n DO
BEGIN
c[1,i] ← TTHREAD:ANGLES[p][joint];
c[3,i] ← TTHREAD:STIME[p];
p ← TTHREAD:NEXT[p];
END;
! Take care of velocities at endpoints;
IF (MEMLOC(TTHREAD:VELS[FIRST],INTEGER)≠0) THEN c[2,1]←TTHREAD:VELS[FIRST][JOINT];
IF (MEMLOC(TTHREAD:VELS[LAST],INTEGER)≠0) THEN c[2,n]←TTHREAD:VELS[LAST][JOINT];
c[3,1] ← 0;
c[4,1] ← 1; ! Slope prescribed at left end;
FOR m ← 2 TIL n DO ! Compute first differences;
c[4,m] ← (c[1,m] - c[1,m-1]) / c[3,m];
IF n = 2 THEN ! Special case if no intermediate points;
BEGIN
dtau ← c[3,2];
divdf3 ← c[2,1] + c[2,2] - 2*c[4,2];
c[3,1] ← (c[4,2] - c[2,1] - divdf3) * dtau;
c[4,1] ← divdf3 * dtau;
c[2,1] ← c[2,1] * dtau;
FOR i ← 1 TIL 4 DO
TTHREAD:COEFF[LAST][JOINT,i-1] ← c[i,1];
RETURN; ! Go on to next joint;
END;
FOR m ← 2 TIL nseg DO ! Generate interior knot eqns & forward pass of Gauss elim;
BEGIN
g ← -c[3,m+1] / c[4,m-1];
c[2,m] ← g * c[2,m-1] + 3 * (c[3,m] * c[4,m+1] + c[3,m+1] * c[4,m]);
c[4,m] ← g * c[3,m-1] + 2 * (c[3,m] + c[3,m+1]);
END;
FOR j ← nseg STEP -1 UNTIL 1 DO ! Carry out back substitution;
c[2,j] ← (c[2,j] - c[3,j] * c[2,j+1]) / c[4,j];
P ← TTHREAD:NEXT[FIRST];
FOR i ← 2 TIL n DO ! Generate the cubic coefficents in each interval;
BEGIN ! & Stow away the answer into the coefficient array;
dtau ← c[3,i];
divdf1 ← (c[1,i] - c[1,i-1]) / dtau;
divdf3 ← c[2,i-1] + c[2,i] - 2*divdf1;
c[3,i-1] ← (divdf1 - c[2,i-1] - divdf3) * dtau;
c[4,i-1] ← divdf3 * dtau;
c[2,i-1] ← c[2,i-1] * dtau;
FOR j ← 1 TIL 4 DO TTHREAD:COEFF[P][JOINT,j-1] ← c[j,i-1];
P ← TTHREAD:NEXT[P];
END;
END "cubspl";
! Main body of TRJCLC starts here;
RPTR(TTHREAD)
MOTION, ! The entire motion will be stored on this thread;
LEADTHREAD, ! A forward pointer used in scanning down thread;
CURTHREAD, OLDTHREAD; ! Used to trace down the motion;
REAL ARRAY DEL [1:14]; ! Joint angle differences;
RANY P, Q, ! Used in tracking down cell links;
TMPPLACE; ! Used as a temp in location calculations;
RPTR(VARIABLE) VAR;
REAL UT, ST, TT; ! User-defined time, system-computed time, total time;
INTEGER M, ! Holds modes;
FLAG, ! Boolean, for success parameters;
JOINT, ! For loop control;
I, ! For loop control;
LOJOINT, HIJOINT, ! Defines which arm;
ARM, ! Mechanism bits for the device used, eg YARM_MECH;
SBITS, ! Status bits for the device used, eg YARMSB;
SEGCNT, ! How many segments to the motion;
LAB, ! For code emission: a label;
DPTR, SEGLEN; ! For code emission: pointers into DATA and RELOC;
INTEGER ARRAY DATA, RELOC [0:1000]; ! Used for emitting code;
BOOLEAN FIRST_SEG;
REAL SLACK;
DEVBITS(ARM,SBITS,LOJOINT,HIJOINT,MOVE$:CF[MOV]);
SEGCNT ← 0;
TT ← 0.;
! Establish the speed factor for this move;
SPEED ← MOVE$:SFAC[MOV]; ! Bound by WLDMOD;
IF SPEED < 1 THEN
BEGIN "too fast"
PRINT("Speed_factor for MOVE < 1, will use it - good luck"&crlf);
! SPEED ← 1; ! Used to enforce speed ≥ 1;
END "too fast";
! Initialize the first node of the motion;
MOTION ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[MOTION] ← MOVE$:CFVAL[MOV]; ! active arm's current frame;
TTHREAD:MODE[MOTION] ← ENDP_MODE; ! endpoint;
NewArray(REAL,TTHREAD:ANGLES[MOTION],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:VELS[MOTION],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[MOTION],FLAG);
IF ¬FLAG THEN COMERR("Illegal start point",MOV);
PLACESOL(TTHREAD:ANGLES[MOTION],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("The initial location is not accessible.
The closest reasonable point is being used.",MOV);
CURTHREAD ← MOTION;
FIRST_SEG ← TRUE; ! Next segment will be first;
! Now add some slack time to the first segment, so it can get moving;
SLACK ← IF SPEED < 2 THEN 0.15 * (2.0 - SPEED) ELSE 0;
! Treat the departure;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(DEPARTURE)) DO P ← CELL:CDR[P];
IF (P ≠ RNULL) ∧ (DEPARTURE:THRU[CELL:CAR[P]] ≠ NILDEPROACH)
THEN BEGIN "depart" ! Won't work for fingers, of course;
IF LOJOINT = HIJOINT THEN COMERR("No deproaches allowed for fingers");
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← DEPARTURE:ACTPLACE[CELL:CAR[P]];
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal departure point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This departure location is not accessible.
The closest reasonable point is being used.",TTHREAD:PLACE[CURTHREAD]);
IF DEPARTURE:CODE[CELL:CAR[P]] ≠ RNULL THEN
! Set up event to signal for code at DEPARTURE;
TTHREAD:EVENT[CURTHREAD] ←
IF RECTYPE(DEPARTURE:CODE[CELL:CAR[P]])=LOC(CMON)
THEN CMON:CONDITION[DEPARTURE:CODE[CELL:CAR[P]]]
ELSE EVDO:VAR[DEPARTURE:CODE[CELL:CAR[P]]];
TTHREAD:STIME[CURTHREAD] ←
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD],FIRST_SEG);
IF SPEED < 2.0 THEN ! Use at least a speed_factor of 2 for deproaches;
TTHREAD:STIME[CURTHREAD] ← TTHREAD:STIME[CURTHREAD] * (2.0 / SPEED);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
TTHREAD:MODE[CURTHREAD] ← DEPA_MODE;
FIRST_SEG ← FALSE; ! Taken care of first segment;
END "depart";
! Put intermediate points into the thread;
P ← MOVE$:CLAUSES[MOV];
WHILE TRUE DO
BEGIN "interm"
! This loop is terminated by a DONE. Each iteration looks at
the next via on the clauses list;
RPTR(VIA) VIAP;
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(VIA)) DO
P ← CELL:CDR[P];
IF P = RNULL THEN DONE "interm";
VIAP ← CELL:CAR[P];
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
IF VIA:CODE[VIAP] ≠ RNULL THEN ! Set up event to signal for code at VIA;
TTHREAD:EVENT[CURTHREAD] ←
IF RECTYPE(VIA:CODE[VIAP])=LOC(CMON)
THEN CMON:CONDITION[VIA:CODE[VIAP]]
ELSE EVDO:VAR[VIA:CODE[VIAP]];
TTHREAD:PLACE[CURTHREAD] ← VIA:ACTPLACE[VIAP];
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal via point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This via location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(TTHREAD:PLACE[CURTHREAD],RNULL)));
IF VIA:VELOC[VIAP] ≠ RNULL
THEN BEGIN ! There is a velocity specification here;
IF LOJOINT = HIJOINT
THEN TTHREAD:VELS[CURTHREAD][LOJOINT]
← SVAL:VAL[SPLANVAL(VELOCITY:VELOC[VIA:VELOC[VIAP]])]
ELSE BEGIN "fvel"
RPTR(V3ECT) VTEMP,V2TEMP; ! To hold offset vector (inches/second);
RPTR(TRANS) FTEMP; ! To hold frame value;
REAL ARRAY OFFANG [1:6]; ! Offset angles;
INTEGER I; ! Loop control;
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
FTEMP ← IF RECTYPE(TMPPLACE)=LOC(FRAME) THEN FRAME:VAL[TMPPLACE]
ELSE TMPPLACE;
VTEMP ← VPLANVAL(VELOCITY:VELOC[VIA:VELOC[VIAP]]);
VTEMP ← V3ADD(TRANS:P[FTEMP],SVMUL(0.2,VTEMP)); ! Add in offset vector;
ARRBLT(OFFANG[1],TTHREAD:ANGLES[CURTHREAD][LOJOINT],6);
! So the out-of bounds result will be reasonable;
V2TEMP ← TRANS:P[FTEMP];
TRANS:P[FTEMP] ← VTEMP;
ARMSOL(OFFANG,FTEMP,IF LOJOINT=1 THEN YARM_MECH ELSE BARM_MECH);
TRANS:P[FTEMP] ← V2TEMP;
FOR JOINT ← LOJOINT STEP 1 UNTIL LOJOINT+5 DO
TTHREAD:VELS[CURTHREAD][JOINT] ←
5.*(OFFANG[JOINT-LOJOINT+1]
- TTHREAD:ANGLES[CURTHREAD][JOINT]);
END "fvel";
END;
ST ← TTHREAD:STIME[CURTHREAD] ← SLACK +
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD],FIRST_SEG);
SLACK ← 0; ! Only add slack to first segment;
FIRST_SEG ← FALSE; ! Taken care of first segment;
IF VIA:TIME[VIAP] ≠ RNULL
THEN BEGIN ! The time is constrained;
RPTR(DURATION) DUR;
DUR ← VIA:TIME[VIAP];
UT ← TTHREAD:UTIME[CURTHREAD]
← SVAL:VAL[SPLANVAL(DURATION:TIME[DUR])];
M ← TTHREAD:MODE[CURTHREAD] ← DURATION:TIME_RELN[DUR];
! test for incompatibilites;
IF ST/SPEED > UT ∧ M ≥ 2 THEN
BEGIN
COMERR(
"Cannot satisfy your time request for this segment without danger;
you want "&CVG(UT)&" seconds, and I think you need "&CVG(ST)&"
seconds. Nonetheless, I am using your request.");
IF UT ≤ 0 THEN
BEGIN
COMERR("But I refuse to let you get away with no time at all!");
UT ← ST;
END;
END;
IF (M=1 ∧ ST<UT) ∨ (M=2 ∧ ST>UT) ∨ (M=3 ∧ ST≠UT) THEN
TTHREAD:STIME[CURTHREAD] ← UT;
END;
TT ← TT + TTHREAD:STIME[CURTHREAD];
P ← CELL:CDR[P];
END "interm";
! Treat the approach;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(APPROACH)) DO P ← CELL:CDR[P];
IF (P ≠ RNULL) ∧ (APPROACH:THRU[CELL:CAR[P]] ≠ NILDEPROACH)
THEN BEGIN "approa" ! Will not work for finger operation;
IF LOJOINT = HIJOINT THEN COMERR("No deproaches allowed for fingers");
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← APPROACH:ACTPLACE[CELL:CAR[P]];
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal approach point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This approach location is not accessible.
The closest reasonable point is being used.",TTHREAD:PLACE[CURTHREAD]);
IF APPROACH:CODE[CELL:CAR[P]] ≠ RNULL THEN
! Set up event to signal for code at APPROACH;
TTHREAD:EVENT[CURTHREAD] ←
IF RECTYPE(APPROACH:CODE[CELL:CAR[P]])=LOC(CMON)
THEN CMON:CONDITION[APPROACH:CODE[CELL:CAR[P]]]
ELSE EVDO:VAR[APPROACH:CODE[CELL:CAR[P]]];
TTHREAD:STIME[CURTHREAD] ← SLACK +
! Add some slack time to the last segment, so it can stop;
(IF SPEED < 2 THEN 0.15 * (2.0 - SPEED) ELSE 0.0) +
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD],FIRST_SEG);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
TTHREAD:MODE[CURTHREAD] ← APPR_MODE;
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← MOVE$:DEXP[MOV];
TTHREAD:MODE[CURTHREAD] ← ENDP_MODE;
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal destination point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This destination location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(TTHREAD:PLACE[CURTHREAD],RNULL)));
TTHREAD:STIME[CURTHREAD] ←
RUNTIME(TTHREAD:ANGLES[CURTHREAD],TTHREAD:ANGLES[OLDTHREAD],TRUE);
IF SPEED < 2.0 THEN ! Use at least a speed_factor of 2 for deproaches;
TTHREAD:STIME[CURTHREAD] ← TTHREAD:STIME[CURTHREAD] * (2.0 / SPEED);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
END "approa"
ELSE BEGIN "arrive"
! There is no deproach point, just put in the final point;
SEGCNT ← SEGCNT + 1;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD] ← NEW_RECORD(TTHREAD);
TTHREAD:PLACE[CURTHREAD] ← MOVE$:DEXP[MOV];
TTHREAD:MODE[CURTHREAD] ← ENDP_MODE;
NewArray(REAL,TTHREAD:ANGLES[CURTHREAD],[LOJOINT:HIJOINT]);
NewArray(REAL,TTHREAD:VELS[CURTHREAD],[LOJOINT:HIJOINT]);
TMPPLACE ← PPLANVAL(TTHREAD:PLACE[CURTHREAD],FLAG);
IF ¬FLAG THEN COMERR("Illegal destination point",MOV);
PLACESOL(TTHREAD:ANGLES[CURTHREAD],TMPPLACE,ARM,FLAG);
IF FLAG THEN
COMERR("This destination location is not accessible.
The closest reasonable point is being used.",
CONS(MOV,CONS(TTHREAD:PLACE[CURTHREAD],RNULL)));
TTHREAD:STIME[CURTHREAD] ← SLACK +
! Add some slack time to the last segment, so it can stop;
(IF SPEED < 2 THEN 0.15 * (2.0 - SPEED) ELSE 0.0) +
RUNTIME(TTHREAD:ANGLES[OLDTHREAD],TTHREAD:ANGLES[CURTHREAD],TRUE);
TT ← TT + TTHREAD:STIME[CURTHREAD];
NewArray(REAL,TTHREAD:COEFF[CURTHREAD],[LOJOINT:HIJOINT,0:5]);
END "arrive";
! See if there's an ON ARRIVAL clause;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ ¬( (RECTYPE(Q←CELL:CAR[P])=LOC(CMON))
∧ (RECTYPE(Q←CMON:CONDITION[Q])=LOC(VARIABLE))
∧ EQU(".AE",VARIABLE:NAME[Q][1 FOR 3]) )
DO P ← CELL:CDR[P];
IF (P ≠ RNULL) THEN TTHREAD:EVENT[CURTHREAD] ← Q;
! Check for overall time constraints. Fulfil them if possible;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(DURATION)) DO
P ← CELL:CDR[P];
IF P ≠ RNULL
THEN BEGIN "timcst"
! There is a global time constraint. Must try to fulfil it;
REAL AVAIL, CURTIM, FACTOR;
INTEGER M;
UT ← SVAL:VAL[SPLANVAL(DURATION:TIME[CELL:CAR[P]])];
IF UT>TT ∧ (DURATION:TIME_RELN[CELL:CAR[P]] LAND '1) ! (> or =);
THEN BEGIN "stretch" ! Easy case;
AVAIL ← 0.; ! Restricted extra time;
CURTIM ← 0; ! Currently used unrestricted time;
CURTHREAD ← TTHREAD:NEXT[MOTION]; ! First segment has no time;
WHILE CURTHREAD ≠ RNULL DO
BEGIN "timchk"
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN ! Time in this segment not inviolate;
IF (M←M LAND '3) ≤ 1
THEN CURTIM ← CURTIM + TTHREAD:STIME[CURTHREAD]
ELSE IF M = 2
THEN AVAIL ← AVAIL + TTHREAD:UTIME[CURTHREAD]
- TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END "timchk";
IF CURTIM
THEN BEGIN "dostretch" ! Just modify those segments
which are not inviolate and have mode ≤ 1;
FACTOR ← (CURTIM + UT - TT) / CURTIM;
CURTHREAD ← TTHREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN ! Expand right segments;
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
∧ (M LAND '3) ≤ 1
THEN TTHREAD:STIME[CURTHREAD]
← FACTOR * TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END;
END
ELSE COMERR(
"You want"&CVG(UT)&"seconds for this motion, and I could give you up
to"& CVG(TT+AVAIL) &", but I am only giving you"& CVG(TT)
&"instead.");
END "stretch"
ELSE IF UT<TT ∧ (DURATION:TIME_RELN[CELL:CAR[P]] LAND '3) ≥ 2 ! (< or =);
THEN BEGIN "shrink" ! Tough case;
IF UT < TT/SPEED THEN COMERR(
"You want only" & CVG(UT) & "for this motion, and I think you need
" & CVG(TT) &". In order to satisfy your request, I am disregarding any
other time constraints you may have placed on the motion.");
CURTIM ← 0; ! Currently used non-inviolate time;
CURTHREAD ← TTHREAD:NEXT[MOTION]; ! First segment has no time;
WHILE CURTHREAD ≠ RNULL DO
BEGIN "timcnt"
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN ! Time in this segment not inviolate;
CURTIM ← CURTIM + TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END "timcnt";
FACTOR ← (CURTIM + UT - TT) / CURTIM;
IF FACTOR ≤ 0
THEN BEGIN
COMERR(
"Your overall time constraint of" & CVG(UT) & "is ridiculous; I am
ignoring it.");
FACTOR ← 1.;
END;
CURTHREAD ← TTHREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN ! Contract right segments;
IF ¬((M←TTHREAD:MODE[CURTHREAD]) LAND INVI_MODE)
THEN TTHREAD:STIME[CURTHREAD]
← FACTOR * TTHREAD:STIME[CURTHREAD];
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END;
END "shrink";
END "timcst";
! Set up the polynomials for any deproach segments;
OLDTHREAD ← MOTION;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "deprs"
REAL DL;
IF TTHREAD:MODE[CURTHREAD] LAND DEPA_MODE THEN
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! stow away the coefficients for this joint.
The poly for each angle is -del*t↑4 + 2*del*t↑3 + initial;
REAL DL;
DL ← TTHREAD:ANGLES[CURTHREAD][JOINT] -
TTHREAD:ANGLES[OLDTHREAD][JOINT];
TTHREAD:COEFF[CURTHREAD][JOINT,4] ← -DL;
TTHREAD:COEFF[CURTHREAD][JOINT,3] ← 2. * DL;
TTHREAD:COEFF[CURTHREAD][JOINT,0] ←
TTHREAD:ANGLES[OLDTHREAD][JOINT];
TTHREAD:VELS[CURTHREAD][JOINT] ←
2. * DL / TTHREAD:STIME[CURTHREAD];
END
ELSE IF TTHREAD:MODE[OLDTHREAD] LAND APPR_MODE THEN
FOR JOINT ← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! stow away the coefficients for this joint. If
del is (final - initial), then the poly for each angle is
{del*t↑4 - 2*del*t↑3 + 2*del*t + initial};
DL ← TTHREAD:ANGLES[CURTHREAD][JOINT] -
TTHREAD:ANGLES[OLDTHREAD][JOINT];
TTHREAD:COEFF[CURTHREAD][JOINT,4] ← DL;
TTHREAD:COEFF[CURTHREAD][JOINT,3] ← -2. * DL;
TTHREAD:COEFF[CURTHREAD][JOINT,1] ← 2. * DL;
TTHREAD:COEFF[CURTHREAD][JOINT,0] ←
TTHREAD:ANGLES[OLDTHREAD][JOINT];
TTHREAD:VELS[OLDTHREAD][JOINT] ←
2. * DL / TTHREAD:STIME[CURTHREAD];
END;
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD];
END "deprs";
! Call the polynomial generator on chunks of the motion.
A chunk contains all segments between two velocity-constrained points.
Moreover we set velocity to 0 for any joints which change direction;
FOR JOINT ← LOJOINT TIL HIJOINT DO
BEGIN "joint"
REAL del,ang;
OLDTHREAD ← MOTION;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD];
ang ← TTHREAD:ANGLES[CURTHREAD][JOINT];
del ← ang - TTHREAD:ANGLES[OLDTHREAD][JOINT];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "chunk"
! Each iteration finds one chunk, brackets it between
OLDTHREAD and CURTHREAD, and makes polys;
INTEGER PNTCNT; ! Counts number of points in each chunk;
PNTCNT ← 2; ! Count the end nodes this way;
WHILE (MEMLOC(TTHREAD:VELS[CURTHREAD],INTEGER) = 0) ∧
del * (TTHREAD:ANGLES[TTHREAD:NEXT[CURTHREAD]][joint] - ang) ≥ 0 DO
BEGIN ! This chunk includes node pointed to by CURTHREAD;
PNTCNT ← PNTCNT + 1;
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
del ← TTHREAD:ANGLES[CURTHREAD][JOINT] - ang;
ang ← TTHREAD:ANGLES[CURTHREAD][JOINT];
END;
! Now OLDTHREAD and CURTHREAD point to nodes on each end of chunk;
IF PNTCNT > 2 ∨
¬(TTHREAD:MODE[CURTHREAD] LAND DEPA_MODE ∨
TTHREAD:MODE[OLDTHREAD] LAND APPR_MODE)
THEN CUBSPL(OLDTHREAD,CURTHREAD,JOINT,PNTCNT-1);
OLDTHREAD ← CURTHREAD;
CURTHREAD ← TTHREAD:NEXT[OLDTHREAD];
END "chunk";
END "joint";
! Compute the gravity and inertia terms;
CURTHREAD ← TTHREAD:NEXT[MOTION];
WHILE CURTHREAD ≠ RNULL DO
BEGIN "grav"
REQUIRE "BEJCZY.REL[AL,HE]" LOAD_MODULE;
REQUIRE "FAITRG.REL[AL,HE]" LOAD_MODULE;
EXTERNAL PROCEDURE DTERMS(REFERENCE REAL RES, ANG; INTEGER ARM);
NewArray(REAL,TTHREAD:GRAVIN[CURTHREAD],[2*LOJOINT:2*HIJOINT+1]);
DTERMS(TTHREAD:GRAVIN[CURTHREAD][2*LOJOINT],
TTHREAD:ANGLES[CURTHREAD][LOJOINT], ARM);
CURTHREAD ← TTHREAD:NEXT[CURTHREAD];
END "grav";
! Output the motion table;
! The format for the coefficients is described in ARM.PAL[11,BES].
SERVO BIT WORD
YARMSB, YHANDSB, BARMSB, or BHANDSB
SERVO BIT WORD
COMMAND BITS; DEFINE NONULLCB = "'1"; ! No end null;
DEFINE WOBBLECB = "'2"; ! Wobble at end;
DEFINE DEPARTCB = "'4"; ! ∃ Departure point;
DEFINE RTMOVECB = "'10"; ! Experimental move
WOBBLE VALUE POINTER Pointer to wobble value for this motion (constant)
RELATIVE SEGMENT PTR Length (bytes) of first segment table
8 + 32*njoints. 0 means there are no more
segments. (Put at very end of table)
TIME milliseconds for this segment
TRANS ptr to transform:
L-O Level-offset of trans (or scalar)
VAL Place for validity number - no longer used
CODE ptr to event to be signalled at end
of this segment
A0 coeff (floating) first joint
:
A5 last coeff, first joint
A0 first coeff, second jolint
:
:
:
A5 last coeff, last joint
NCI final joint gravity loading, first joint
NCII final joint inertia loading, first joint
:
:
NCI final joint gravity loading, last joint
NCII final joint inertia loading, last joint
RELATIVE SEGMENT PTR
This is the format for the transform-validity list:
T1 level-offset of transform for first arm
0 room for validity number
:
Tn level-offset of transform for last arm
0 room for validity number
DATA and RELOC are used to output the motion table. Each of
these is 1000 long (very long motions may not fit.). See
EMITER.HDR for the pseudo-op definitions.
;
ARRCLR(RELOC);
ARRCLR(DATA);
! Output pseudo-op for motion;
LAB ← GENLABEL; ! Points to motion table;
EMIT(PSDCODE,MOVE_PSOP,PSINST);
EMIT(PSDCODE,LAB,SYMREF);
EMIT(PSDCODE,ARM,CONST);
! EMIT(PSDCODE,ERROR_BITS,CONST) - in PASS3 - bit mask for error handler;
! EMIT(PSDCODE,LAB2,SYMREF) - in PASS3 - tells us where to go for next pcode;
! EMIT(PSDCODE,LAB1,SYMREF) - in PASS3 - tells us where to go for a retry;
! Output trajectory file;
MAKE_REMARK(TJFILE,"Motion table");
EMIT(TJFILE,LAB,SYMDEC);
! Check for nulling; ! No_nulling is once again the default;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(NNULL)) DO P ← CELL:CDR[P];
IF (P=RNULL) ∨ (NNULL:FLAG[CELL:CAR[P]]) THEN DATA[6] ← NONULLCB;
! Check for run-time move;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(RTMOVE)) DO P ← CELL:CDR[P];
IF (P≠RNULL) THEN DATA[6] ← DATA[6] LOR RTMOVECB;
! Check for wobble;
P ← MOVE$:CLAUSES[MOV];
WHILE (P≠RNULL) ∧ (RECTYPE(CELL:CAR[P])≠LOC(WOBBLE)) DO P ← CELL:CDR[P];
DATA[4] ← SBITS; ! servo bits;
RELOC[4] ← CONST;
DATA[5] ← 0; ! No second servo bits;
DATA[6] ← ! want DEPARTCB only if this motion has a deproach;
DATA[6] LOR ! this already contains the desired nulling bit;
(IF TTHREAD:MODE[TTHREAD:NEXT[MOTION]] LAND DEPA_MODE THEN DEPARTCB ELSE 0)
LOR (IF P≠RNULL THEN WOBBLECB ELSE 0);
RELOC[5] ← RELOC[6] ← CONST;
EMIT(TJFILE,DATA[4],RELOC[4],3); ! Label, servo bits, servo bits, command bits;
IF P≠RNULL THEN ! Emit pointer to wobble value & the actual value;
BEGIN
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Point to the wobble value;
MAKE_REMARK(SMLBLK,"Wobble value for motion");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the wobble value;
DATA[7] ← MEMORY[LOC(SVAL:VAL[WOBBLE:VAL[CELL:CAR[P]]]),INTEGER];
RELOC[7] ← FLOAT;
EMIT(SMLBLK,DATA[7],RELOC[7],1);
END
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of a wobble value;
SEGLEN ← 8 + 32*(IF (ARM = YARM_MECH) OR (ARM = BARM_MECH) THEN 6 ELSE 1);
DATA[7] ← SEGLEN; RELOC[7] ← CONST;
Q ← TTHREAD:NEXT[MOTION];
WHILE Q ≠ RNULL DO
BEGIN "coeout" ! Coefficients for one segment;
INTEGER PLACETYPE; ! One of EXPRN, VALU$, VARIABLE;
P ← Q;
Q ← TTHREAD:NEXT[Q];
IF TTHREAD:PLACE[P] = RNULL THEN CONTINUE "coeout";
! Avoid outputting the short segments that end at
unconstrained points;
MAKE_REMARK(TJFILE,"Relative segment pointer");
DATA[8] ← TTHREAD:STIME[P]*1000.; RELOC[8] ← CONST; ! Milliseconds;
EMIT(TJFILE,DATA[7],RELOC[7],2); ! Relative seg ptr, time;
PLACETYPE ← RECTYPE(DEXPR:EXPN[TTHREAD:PLACE[P]]);
IF PLACETYPE=LOC(VARIABLE) OR PLACETYPE=LOC(EXPRN)
THEN BEGIN "needtrans"
! There is an associated place, need a trans pointer;
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Refer to the trans pointer;
MAKE_REMARK(SMLBLK,"Trans pointer for motion");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the trans pointer;
EMIT(SMLBLK,VARIABLE:OFFSET[DEXPR:VAR[TTHREAD:PLACE[P]]],CONST,1);
! Point to the trans itself;
! Currently only one frame allowed;
EMIT(SMLBLK,0,CONST,1); ! Leave room for the validity bit;
END "needtrans"
ELSE EMIT(TJFILE,0,CONST,1); ! The absence of a trans pointer;
IF TTHREAD:EVENT[P]≠RNULL
THEN BEGIN ! There is some associated code, need to signal to it.;
EMIT(TJFILE,LAB←GENLABEL,SYMREF,1); ! Refer to the event pointer;
MAKE_REMARK(SMLBLK,"Event pointer for VIA code");
EMIT(SMLBLK,LAB,SYMDEC,1); ! Here is the event pointer;
EMIT(SMLBLK,VARIABLE:OFFSET[TTHREAD:EVENT[P]],CONST,1);
! Point to the event itself;
EMIT(SMLBLK,0,CONST,1); ! Leave room for the armcode to use;
END
ELSE
EMIT(TJFILE,0,CONST,1); ! The absence of associated code;
DPTR ← 9;
MAKE_REMARK(TJFILE,"Coefficients, gravity, inertia");
! Coefficients;
FOR JOINT← LOJOINT STEP 1 UNTIL HIJOINT DO
BEGIN ! Each iteration spits out the coefficient of one joint;
INTEGER DEGR;
FOR DEGR ← 0 STEP 1 UNTIL 5 DO
DATA[DPTR+DEGR] ← MEMORY[LOC(TTHREAD:COEFF[P][JOINT,DEGR]),INTEGER];
DPTR ← DPTR + 6;
END;
! The gravity and inertia terms;
FOR I ← 2*LOJOINT STEP 1 UNTIL 2*HIJOINT+1 DO
BEGIN ! Each 2 iterations spits out the terms for one joint;
DATA[DPTR] ← MEMORY[LOC(TTHREAD:GRAVIN[P][I]),INTEGER];
DPTR ← DPTR+1;
END;
RELOC[9] ← FLOAT; ! They are all floating point constants;
ARRBLT(RELOC[10],RELOC[9],DPTR-10);
EMIT(TJFILE,DATA[9],RELOC[9],DPTR-9); ! All the coefficients for this seg;
END "coeout";
EMIT(TJFILE,0,CONST,1); ! The last relative segment pointer is 0;
MAKE_REMARK(TJFILE,"End of motion table");
! Reclaim all the arrays in the motion thread;
Q ← MOTION;
WHILE Q ≠ RNULL DO
BEGIN "reclaim"
INTEGER ADR;
EXTERNAL PROCEDURE ARYEL (INTEGER ADRESS); ! In the SAIL segment;
ADR ← 0;
MEMLOC(TTHREAD:ANGLES[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(TTHREAD:VELS[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(TTHREAD:COEFF[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
ADR ← 0;
MEMLOC(TTHREAD:GRAVIN[Q],INTEGER) ↔ ADR;
IF ADR THEN ARYEL(ADR);
Q ← TTHREAD:NEXT[Q];
END "reclaim";
! End of TRJCLC;
END "trjclc";
! CENTCLC, STOPCLC;
INTERNAL PROCEDURE CENTCLC (RPTR(CENTER) CNTR);
BEGIN "centclc"
! The "trajectory" table looks like this:
COFLST: XXXXXX TWO SERVO BIT WORDS, 7 BITS MUST BE ON, A HAND
XXXXXX SERVO AND ALL JOINT SERVOS OF THE SAME ARM
0 NO COMMAND BITS
0 NO WOBBLE VALUE
0 NO NEXT SEGMENT
0 NO FUNCTION TIME
0 NO TRANSFORM
CODE PTR TO CODE TO BE SCHEDULED THIS SEG
NO POLYNOMIAL TO FOLLOW
;
! Does not yet handle any cmons or code;
INTEGER ARM, SBITS, LAB;
INTEGER LOJOINT, HIJOINT; ! Not used;
PRELOAD_WITH 0,0,0,0,0,0;
OWN INTEGER ARRAY ZEROS[1:6];
PRELOAD_WITH CONST,CONST,CONST,CONST,CONST,CONST;
OWN INTEGER ARRAY CONSTS[1:6];
DEVBITS(ARM,SBITS,LOJOINT,HIJOINT,CENTER:CF[CNTR]);
! Want to turn on the hand bits as well:
IF ARM = YARM_MECH
THEN BEGIN
ARM ← YARM_MECH + YHAND_MECH
SBITS ← YARMSB + YHANDSB
END
ELSE IF ARM = BARM_MECH
THEN BEGIN
ARM ← BARM_MECH + BHAND_MECH
SBITS ← BARMSB + BHANDSB
END;
! Do it like this:;
ARM ← ARM LOR (ARM LSH 1);
SBITS ← SBITS LOR (SBITS LSH -1);
EMIT(PSDCODE,CENTER_PSOP,PSINST);
LAB ← GENLABEL;
EMIT(PSDCODE,LAB,SYMREF);
EMIT(PSDCODE,ARM,CONST);
! EMIT(PSDCODE,LAB1,SYMREF) - in PASS3 - tells us where to go for a retry;
MAKE_REMARK(TJFILE,"Center table");
EMIT(TJFILE,LAB,SYMDEC);
EMIT(TJFILE,SBITS,CONST);
EMIT(TJFILE,ZEROS[1],CONSTS[1],6);
END "centclc";
INTERNAL PROCEDURE STOPCLC(RPTR(STOP) STP);
BEGIN "stopclc"
INTEGER ARM, SBITS, LOJOINT, HIJOINT; ! Only ARM is used;
DEVBITS(ARM,SBITS,LOJOINT, HIJOINT, STOP:CF[STP]);
EMIT(PSDCODE,STOP_PSOP,PSINST);
EMIT(PSDCODE,ARM,CONST);
END "stopclc";
END $$prgid;
! Bugs
;